#include "dm_motor.h"

DM_Motor_groupTypeDef DM_MOTOR[8];
float motor_ctrl_data[8][9] = 
{
    {0.0, 1.0, 0.0, 0.03, 1.0, 0.02, 12.5, 30.0, 10.0},
    {0.0, 1.0, 0.0, 0.03, 1.0, 0.02, 12.5, 30.0, 10.0},
    {0.0, 1.0, 0.0, 0.03, 1.0, 0.02, 12.5, 30.0, 10.0},
    {0.0, 1.0, 0.0, 0.03, 1.0, 0.02, 12.5, 30.0, 10.0},
    {0.0, 1.0, 0.0, 0.03, 1.0, 0.02, 12.5, 30.0, 10.0},
    {0.0, 1.0, 0.0, 0.03, 1.0, 0.02, 12.5, 30.0, 10.0},
    {0.0, 1.0, 0.0, 0.03, 1.0, 0.02, 12.5, 30.0, 10.0},
    {0.0, 1.0, 0.0, 0.03, 1.0, 0.02, 12.5, 30.0, 10.0}
};
/* 1.pos_set 2.vel_set 3.tor_set 4.cur_set 5.kp_set 6.kd_set 7.PMAX 8.VMAX 9.TMAX  */

/* Includes ------------------------------------------------------------------*/
void dm_motor_set_para(DM_Motor_groupTypeDef* motor);
void dm_motor_enable(DM_Motor_groupTypeDef* motor);
#ifdef USE_FREERTOS
#include "can_task.h"
#endif

/**
************************************************************************
* @brief:      	dm_callbackdata: 获取DM4310电机反馈数据函数
* @param[in]:   motor:    指向motor_t结构的指针，包含电机相关信息和反馈数据
* @param[in]:   rx_data:  指向包含反馈数据的数组指针
* @retval:     	void
* @details:    	从接收到的数据中提取DM4310电机的反馈信息，包括电机ID、
*               状态、位置、速度、扭矩以及相关温度参数
************************************************************************
**/
void dm_callbackdata(DM_Motor_groupTypeDef* motor, uint8_t* rx_data)
{
	motor->para.id = (rx_data[0]) & 0x0F;
	motor->para.state = (rx_data[0]) >> 4;
	motor->para.p_int = (rx_data[1] << 8) | rx_data[2];
	motor->para.v_int = (rx_data[3] << 4) | (rx_data[4] >> 4);
	motor->para.t_int = ((rx_data[4] & 0xF) << 8) | rx_data[5];
	motor->para.pos = uint_to_float(motor->para.p_int, -motor->motor_data.PMAX, motor->motor_data.PMAX, 16); // (-12.5,12.5)
	motor->para.vel = uint_to_float(motor->para.v_int, -motor->motor_data.VMAX, motor->motor_data.VMAX, 12); // (-45.0,45.0)
	motor->para.tor = uint_to_float(motor->para.t_int, -motor->motor_data.TMAX, motor->motor_data.TMAX, 12);  // (-18.0,18.0)
	motor->para.Tmos = (float)(rx_data[6]);
	motor->para.Tcoil = (float)(rx_data[7]);
}

/**
************************************************************************
* @brief:      	float_to_uint: 浮点数转换为无符号整数函数
* @param[in]:   x_float:	待转换的浮点数
* @param[in]:   x_min:		范围最小值
* @param[in]:   x_max:		范围最大值
* @param[in]:   bits: 		目标无符号整数的位数
* @retval:     	无符号整数结果
* @details:    	将给定的浮点数 x 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个指定位数的无符号整数
************************************************************************
**/
int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
	/* Converts a float to an unsigned int, given range and number of bits */
	float span = x_max - x_min;
	float offset = x_min;
	return (int)((x_float - offset) * ((float)((1 << bits) - 1)) / span);
}

/**
************************************************************************
* @brief:      	uint_to_float: 无符号整数转换为浮点数函数
* @param[in]:   x_int: 待转换的无符号整数
* @param[in]:   x_min: 范围最小值
* @param[in]:   x_max: 范围最大值
* @param[in]:   bits:  无符号整数的位数
* @retval:     	浮点数结果
* @details:    	将给定的无符号整数 x_int 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个浮点数
************************************************************************
**/
float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
	/* converts unsigned int to float, given range and number of bits */
	float span = x_max - x_min;
	float offset = x_min;
	return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}

/**
************************************************************************
* @brief:      	canx_send_data(FDCAN_HandleTypeDef *hfdcan, uint16_t id, uint8_t *data, uint32_t len)
* @param:       hcan: CAN句柄
* @param:       id: 	CAN设备ID
* @param:       data: 发送的数据
* @param:       len:  发送的数据长度
* @retval:     	void
* @details:    	发送数据
************************************************************************
**/
void canx_send_data(CAN_HandleTypeDef* hcan, uint16_t id, uint8_t* data, int8_t size)
{
#ifdef USE_FREERTOS
	can_message msg;
	msg.tx_header.StdId = id;
	msg.tx_header.IDE = CAN_ID_STD;
	msg.tx_header.RTR = CAN_RTR_DATA;
	msg.tx_header.DLC = size;
	msg.tx_header.TransmitGlobalTime = DISABLE;
	memcpy(msg.data, data, size);
	if (hcan == &hcan2)
	{
		extern QueueHandle_t can2MessageQueueHandle;
		xQueueSendToBack(can2MessageQueueHandle, &msg, 100);
	}
#else
	uint32_t CAN_TX_BOX0;
	CAN_TxHeaderTypeDef tx_header;
	tx_header.StdId = id;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = size;
	tx_header.TransmitGlobalTime = DISABLE;
	HAL_CAN_AddTxMessage(hcan, &tx_header, data, &CAN_TX_BOX0);
#endif
}

/**
************************************************************************
* @brief:      	dm4310_motor_init: DM4310电机初始化函数
* @param:      	void
* @retval:     	void
* @details:    	初始化1个DM4310型号的电机，设置默认参数和控制模式
*               设置ID、控制模式和命令模式等
************************************************************************
**/
void dm_motor_init(CAN_HandleTypeDef* hcan, DM_Motor_groupTypeDef* motor, uint16_t id, uint16_t mst_id, uint8_t mode)
{
	// 设置Motor的电机
	motor->hcan = hcan;
	motor->id = id;
	motor->mst_id = mst_id;
	motor->motor_data.read_flag = 1;
	motor->motor_ctrl.mode = mode;
	motor->motor_ctrl.vel_set = motor_ctrl_data[id][0];
	motor->motor_ctrl.pos_set = motor_ctrl_data[id][1];
	motor->motor_ctrl.tor_set = motor_ctrl_data[id][2];
	motor->motor_ctrl.cur_set = motor_ctrl_data[id][3];
	motor->motor_ctrl.kp_set = motor_ctrl_data[id][4];
	motor->motor_ctrl.kd_set = motor_ctrl_data[id][5];
	motor->motor_data.PMAX = motor_ctrl_data[id][6];
	motor->motor_data.VMAX = motor_ctrl_data[id][7];
	motor->motor_data.TMAX = motor_ctrl_data[id][8];
	dm_motor_enable(motor);
}

/**
************************************************************************
* @brief:      	enable_motor_mode: 启用电机模式函数
* @param[in]:   hcan:     指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要开启的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送启用特定模式的命令
************************************************************************
**/
void enable_motor_mode(CAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint8_t data[8];
	uint16_t id = motor_id + mode_id;

	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFC;

	canx_send_data(hcan, id, data, 8);
}

/**
************************************************************************
* @brief:      	disable_motor_mode: 禁用电机模式函数
* @param[in]:   hcan:     指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要禁用的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送禁用特定模式的命令
************************************************************************
**/
void disable_motor_mode(CAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint8_t data[8];
	uint16_t id = motor_id + mode_id;

	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFD;

	canx_send_data(hcan, id, data, 8);
}

/**
************************************************************************
* @brief:      	clear_err: 清除电机错误函数
* @param[in]:   hcan:     指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要清除错误的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送清除错误的命令。
************************************************************************
**/
void clear_err(CAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint8_t data[8];
	uint16_t id = motor_id + mode_id;

	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFB;

	canx_send_data(hcan, id, data, 8);
}

/**
************************************************************************
* @brief:      	save_pos_zero: 保存位置零点函数
* @param[in]:   hcan:     指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要保存位置零点的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送保存位置零点的命令
************************************************************************
**/
void save_pos_zero(CAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint8_t data[8];
	uint16_t id = motor_id + mode_id;

	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFE;

	canx_send_data(hcan, id, data, 8);
}

/**
************************************************************************
* @brief:      	dm4310_clear: 清除DM4310电机控制参数函数
* @param[in]:   motor:   指向motor_t结构的指针，包含电机相关信息和控制参数
* @retval:     	void
* @details:    	将DM4310电机的命令参数和控制参数清零，包括位置、速度、
*               比例增益(KP)、微分增益(KD)和扭矩
************************************************************************
**/
void dm_motor_clear_para(DM_Motor_groupTypeDef* motor)
{
	motor->motor_ctrl.kp_set = 0;
	motor->motor_ctrl.kd_set = 0;
	motor->motor_ctrl.pos_set = 0;
	motor->motor_ctrl.vel_set = 0;
	motor->motor_ctrl.tor_set = 0;
	motor->motor_ctrl.cur_set = 0;
}

/**
************************************************************************
* @brief:      	dm4310_enable: 启用DM4310电机控制模式函数
* @param[in]:   hcan:    指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor:   指向motor_t结构的指针，包含电机相关信息和控制参数
* @retval:     	void
* @details:    	根据电机控制模式启用相应的模式，通过CAN总线发送启用命令
*               支持的控制模式包括位置模式、位置速度控制模式和速度控制模式
************************************************************************
**/
void dm_motor_enable(DM_Motor_groupTypeDef* motor)
{
	switch (motor->motor_ctrl.mode)
	{
	case mit_mode:
		enable_motor_mode(motor->hcan, motor->id, MIT_MODE);
		break;
	case pos_mode:
		enable_motor_mode(motor->hcan, motor->id, POS_MODE);
		break;
	case spd_mode:
		enable_motor_mode(motor->hcan, motor->id, SPD_MODE);
		break;
	case psi_mode:
		enable_motor_mode(motor->hcan, motor->id, PSI_MODE);
		break;
	}
}

/**
************************************************************************
* @brief:      	dm4310_disable: 禁用DM4310电机控制模式函数
* @param[in]:   hcan:    指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor:   指向motor_t结构的指针，包含电机相关信息和控制参数
* @retval:     	void
* @details:    	根据电机控制模式禁用相应的模式，通过CAN总线发送禁用命令
*               支持的控制模式包括位置模式、位置速度控制模式和速度控制模式
************************************************************************
**/
void dm_motor_disable(DM_Motor_groupTypeDef* motor)
{
	switch (motor->motor_ctrl.mode)
	{
	case mit_mode:
		disable_motor_mode(motor->hcan, motor->id, MIT_MODE);
		break;
	case pos_mode:
		disable_motor_mode(motor->hcan, motor->id, POS_MODE);
		break;
	case spd_mode:
		disable_motor_mode(motor->hcan, motor->id, SPD_MODE);
		break;
	case psi_mode:
		disable_motor_mode(motor->hcan, motor->id, PSI_MODE);
		break;
	}
	dm_motor_clear_para(motor);
}

/**
************************************************************************
* @brief:      	dm4310_clear_err: 清除DM4310电机错误函数
* @param[in]:   hcan: 	 指向CAN控制结构体的指针
* @param[in]:  	motor:   指向电机结构体的指针
* @retval:     	void
* @details:    	根据电机的控制模式，调用对应模式的清除错误函数
************************************************************************
**/
void dm_motor_clear_err(DM_Motor_groupTypeDef* motor)
{
	switch (motor->motor_ctrl.mode)
	{
	case mit_mode:
		clear_err(motor->hcan, motor->id, MIT_MODE);
		break;
	case pos_mode:
		clear_err(motor->hcan, motor->id, POS_MODE);
		break;
	case spd_mode:
		clear_err(motor->hcan, motor->id, SPD_MODE);
		break;
	case psi_mode:
		clear_err(motor->hcan, motor->id, PSI_MODE);
		break;
	}
}

/**
************************************************************************
* @brief:      	dm_save_pos_zero: 保存位置零点函数
* @param[in]:   hcan:     指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   mode_id:  模式ID，指定要保存位置零点的模式
* @retval:     	void
* @details:    	通过CAN总线向特定电机发送保存位置零点的命令
************************************************************************
**/
void dm_motor_save_pos_zero(DM_Motor_groupTypeDef* motor)
{
	switch (motor->motor_ctrl.mode)
	{
	case mit_mode:
		save_pos_zero(motor->hcan, motor->id, MIT_MODE);
		break;
	case pos_mode:
		save_pos_zero(motor->hcan, motor->id, POS_MODE);
		break;
	case spd_mode:
		save_pos_zero(motor->hcan, motor->id, SPD_MODE);
		break;
	case psi_mode:
		save_pos_zero(motor->hcan, motor->id, PSI_MODE);
		break;
	}
}

/**
************************************************************************
* @brief:      	mit_ctrl: MIT模式下的电机控制函数
* @param[in]:   hcan:			指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id:	电机ID，指定目标电机
* @param[in]:   pos:			位置给定值
* @param[in]:   vel:			速度给定值
* @param[in]:   kp:				位置比例系数
* @param[in]:   kd:				位置微分系数
* @param[in]:   torq:			转矩给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送MIT模式下的控制帧。
************************************************************************
**/
void mit_ctrl(DM_Motor_groupTypeDef* motor, uint16_t motor_id, float pos, float vel, float kp, float kd, float torq)
{
	uint8_t data[8];
	uint16_t pos_tmp, vel_tmp, kp_tmp, kd_tmp, tor_tmp;
	uint16_t id = motor_id + MIT_MODE;

	pos_tmp = float_to_uint(pos, -motor->motor_data.PMAX, motor->motor_data.PMAX, 16);
	vel_tmp = float_to_uint(vel, -motor->motor_data.VMAX, motor->motor_data.VMAX, 12);
	kp_tmp = float_to_uint(kp, KP_MIN, KP_MAX, 12);
	kd_tmp = float_to_uint(kd, KD_MIN, KD_MAX, 12);
	tor_tmp = float_to_uint(torq, -motor->motor_data.TMAX, motor->motor_data.TMAX, 12);

	data[0] = (pos_tmp >> 8);
	data[1] = pos_tmp;
	data[2] = (vel_tmp >> 4);
	data[3] = ((vel_tmp & 0xF) << 4) | (kp_tmp >> 8);
	data[4] = kp_tmp;
	data[5] = (kd_tmp >> 4);
	data[6] = ((kd_tmp & 0xF) << 4) | (tor_tmp >> 8);
	data[7] = tor_tmp;

	canx_send_data(motor->hcan, id, data, 8);
}

/**
************************************************************************
* @brief:      	pos_speed_ctrl: 位置速度控制函数
* @param[in]:   hcan:			指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id:	电机ID，指定目标电机
* @param[in]:   vel:			速度给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送位置速度控制命令
************************************************************************
**/
void pos_spd_ctrl(DM_Motor_groupTypeDef* motor, uint16_t motor_id, float pos, float vel)
{
	uint16_t id;
	uint8_t* pbuf, * vbuf;
	uint8_t data[8];

	id = motor_id + POS_MODE;
	pbuf = (uint8_t*)&pos;
	vbuf = (uint8_t*)&vel;

	data[0] = *pbuf;
	data[1] = *(pbuf + 1);
	data[2] = *(pbuf + 2);
	data[3] = *(pbuf + 3);

	data[4] = *vbuf;
	data[5] = *(vbuf + 1);
	data[6] = *(vbuf + 2);
	data[7] = *(vbuf + 3);

	canx_send_data(motor->hcan, id, data, 8);
}

/**
************************************************************************
* @brief:      	speed_ctrl: 速度控制函数
* @param[in]:   hcan: 		指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   vel: 			速度给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送速度控制命令
************************************************************************
**/
void spd_ctrl(DM_Motor_groupTypeDef* motor, uint16_t motor_id, float vel)
{
	uint16_t id;
	uint8_t* vbuf;
	uint8_t data[4];

	id = motor_id + SPD_MODE;
	vbuf = (uint8_t*)&vel;

	data[0] = *vbuf;
	data[1] = *(vbuf + 1);
	data[2] = *(vbuf + 2);
	data[3] = *(vbuf + 3);

	canx_send_data(motor->hcan, id, data, 4);
}

/**
************************************************************************
* @brief:      	position_ctrl: 位置控制函数
* @param[in]:   hcan: 		指向CAN_HandleTypeDef结构的指针，用于指定CAN总线
* @param[in]:   motor_id: 电机ID，指定目标电机
* @param[in]:   vel: 			速度给定值
* @retval:     	void
* @details:    	通过CAN总线向电机发送速度控制命令
************************************************************************
**/
void pos_ctrl(DM_Motor_groupTypeDef* motor, uint16_t motor_id, float pos, float vel, float cur)
{
	uint16_t id;
	uint8_t* pbuf, * vbuf, * ibuf;
	uint8_t data[8];

	uint16_t u16_vel = vel * 100;
	uint16_t u16_cur = cur * 10000;

	id = motor_id + PSI_MODE;
	pbuf = (uint8_t*)&pos;
	vbuf = (uint8_t*)&u16_vel;
	ibuf = (uint8_t*)&u16_cur;

	data[0] = *pbuf;
	data[1] = *(pbuf + 1);
	data[2] = *(pbuf + 2);
	data[3] = *(pbuf + 3);
	data[4] = *vbuf;
	data[5] = *(vbuf + 1);
	data[6] = *ibuf;
	data[7] = *(ibuf + 1);

	canx_send_data(motor->hcan, id, data, 8);
}

/**
************************************************************************
* @brief:      	dm4310_ctrl: 发送DM4310电机控制命令函数
* @param[in]:   hcan:    指向CAN_HandleTypeDef结构的指针
* @param[in]:   motor:   指向motor_t结构的指针，包含电机相关信息和控制参数
* @retval:     	void
* @details:    	根据电机控制模式发送相应的命令到DM4310电机
*               支持的控制模式包括位置模式、位置速度控制模式和速度控制模式
************************************************************************
**/
void dm_motor_ctrl(DM_Motor_groupTypeDef* motor)
{
	switch (motor->motor_ctrl.mode)
	{
		case mit_mode:
			mit_ctrl(motor, motor->id, motor->motor_ctrl.pos_set, motor->motor_ctrl.vel_set, motor->motor_ctrl.kp_set, motor->motor_ctrl.kd_set, motor->motor_ctrl.tor_set);
			break;
		case pos_mode:
			pos_spd_ctrl(motor, motor->id, motor->motor_ctrl.pos_set, motor->motor_ctrl.vel_set);
			break;
		case spd_mode:
			spd_ctrl(motor, motor->id, motor->motor_ctrl.vel_set);
			break;
		case psi_mode:
			pos_ctrl(motor, motor->id, motor->motor_ctrl.pos_set, motor->motor_ctrl.vel_set, motor->motor_ctrl.cur_set);
			break;
	}
}